
#include "TI_CC2500.h"
#include "FreeRTOS.h"
#include "constants.h"
#include "queue.h"
#include "task.h"
#include "serial.h"
#include "semphr.h" // semaphores
#include "packet.h"

#include "uart.h"

extern char rxBuffer[64];

/* The queues used to hold received characters. */
//xQueueHandle xRxedHeart; 
xQueueHandle xRxedData;
//static xQueueHandle xTxedHeart; 
xQueueHandle xTxedData;
//extern xSemaphoreHandle xRx1Semaphore;
//static xSemaphoreHandle xRx2Semaphore;



/*-----------------------------------------------------------*/
xComPortHandle xSerialPortInitCustom( unsigned portBASE_TYPE uxQueueLength )
{
	/* Initialise the hardware. */
	portENTER_CRITICAL();
	{
		/* Create the queues used by the sending tasks. */
		//xRxedHeart = xQueueCreate( uxQueueLength, ( unsigned portBASE_TYPE ) sizeof( char ) );
                xRxedData = xQueueCreate( uxQueueLength, ( unsigned portBASE_TYPE ) sizeof( char ) );
                //xTxedHeart = xQueueCreate( uxQueueLength, ( unsigned portBASE_TYPE ) sizeof( char ) );
                xTxedData = xQueueCreate( uxQueueLength, ( unsigned portBASE_TYPE ) sizeof( PACKET* ) );

		/* Nothing in the buffer yet. */
                //vSemaphoreCreateBinary( xRx2Semaphore  );
                //printHex()
	}
	portEXIT_CRITICAL();
	
	return NULL;
}
/*-----------------------------------------------------------*/
/*-----------------------------------------------------------*/
            /* TRANSMIT FROM QUEUE FUNCTIONS */
/*-----------------------------------------------------------*/

signed portBASE_TYPE xSerialSendPkt( xComPortHandle pxPort, PACKET *pcTxedPkt, portTickType xBlockTime, int pktSize)
{
    portBASE_TYPE xErrorFree = pdFALSE;
    /* Get the next character from the buffer.  Return false if no characters
    are available, or arrive before xBlockTime expires. */

    // Grab Semaphore/Mutex for using the queue here
    
   //putChar('U');
      if( (xQueueSendToBack( xTxedData, pcTxedPkt, 0 )) ){
          //putChar('J');
          xErrorFree = pdTRUE;
      }

    // Release Semaphore/Mutex for using the queue here

    return xErrorFree;
}

signed portBASE_TYPE xSerialGetToSend( xComPortHandle pxPort, PACKET *pcTxedPkt, portTickType xBlockTime )
{
    int i, pktSize=0;
    char length=0;
    PACKET interPkt;
    portBASE_TYPE xErrorFree = pdFALSE;
    /* Get the next character from the buffer.  Return false if no characters
    are available, or arrive before xBlockTime expires. */
    
    // If necessary, grab Semaphore/Mutex for using the queue here
      
      if( (xQueueReceive( xTxedData, pcTxedPkt, xBlockTime )) ){
        //putChar('P');
         
        //printHex();
        xErrorFree = pdTRUE;
      }
    

    // If necessary, release Semaphore/Mutex for using the queue here
    return xErrorFree;
}
/*-----------------------------------------------------------*/


signed portBASE_TYPE xSerialGetDataPkt( xComPortHandle pxPort, char *pcRxedChar, portTickType xBlockTime )
{
    int i, j, pktSize=0, sizeLoc=0;
    char testit;
    //char receive[15];
    portBASE_TYPE xErrorFree = pdTRUE;
    /* Get the next character from the buffer.  Return false if no characters
    are available, or arrive before xBlockTime expires. */

    
    for(i=0; i<4; i++){
        if( xQueueReceive( xRxedData, &testit, xBlockTime ) != pdTRUE ){
          xErrorFree = pdFALSE;
          break;
	} else {
          pcRxedChar[i] = testit;
          if((i==0) && testit==0){
            //println("");
            //print("#BAD-0#");
            sizeLoc++;
            i=0;
          }
          //putChar(pcRxedChar[i]);
        }//end if
    }//end for
    
    if(xErrorFree == pdTRUE){
      pktSize = pcRxedChar[sizeLoc];//mapPKTsize(pcRxedChar[0]);
      if(sizeLoc > 0){
        for(j=0; j<sizeLoc; j++){
          pcRxedChar[0] = pcRxedChar[1];
          pcRxedChar[1] = pcRxedChar[2];
          pcRxedChar[2] = pcRxedChar[3];
          i--;
        }
      }
      //putChar(pktSize);
      if(pktSize == 0){
        xErrorFree = pdFALSE;
        println("");
        print("#BAD-S# -");
        putChar(pcRxedChar[0]);
        print("-|-");
        printHex(pcRxedChar[0]);
        print("- \n");
      }//end if
      //printHex(pcRxedChar[0]);
    }//end if
    for(; i<pktSize; i++){
        if( !(xQueueReceive( xRxedData, &testit, ( ( portTickType ) 200/portTICK_RATE_MS ) )) ){
          //xErrorFree = pdFALSE;
          
          break;
	} else {
          pcRxedChar[i] = testit;
          //putChar(pcRxedChar[i]);
        }//end if
        if(pcRxedChar[i] == '\n'){
          break;
        }
    }
   
    return xErrorFree;
}
/*-----------------------------------------------------------*/

/*
* UART RX interrupt service routine.
*/

//interrupt that handles wireless messages
void rxInterrupt (void) __interrupt[PORT2_VECTOR]{
  int i; 
  char len=64;
  char test;
  portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
    //println("||interrupting||");
  
  if (RFReceivePacket(rxBuffer,&len)){       // Fetch packet from CCxxxx    
    for(i = 0; i <= len; i++) {
      if(i==0){
        test = len; 
      } else {
        test = rxBuffer[i];
      }
      portENTER_CRITICAL();
      {
      if(xQueueSendToBackFromISR( xRxedData, &test, &xHigherPriorityTaskWoken )==errQUEUE_FULL){  // place on the game guess queue
      }
      }
      portEXIT_CRITICAL();
      //}
    }//end for
    

    if( xHigherPriorityTaskWoken )
    {
            //taskYIELD();
    }
  }//end if
  
  P2IFG &= ~0x10;            // Clear flag
}//end rxInterrupt ISR


